﻿#include "CoveragePlan.h"
#include<cmath>
//#include "opencv2/opencv.hpp"

using namespace pi;
using namespace std;
//cv::Mat tmp(cv::Size(1080,1080),CV_8UC3,cv::Scalar(255,255,255));

#define  M_PI 3.14159265358979323846
#define EARTH_RADIUS 6378137.0  ///< Earth radius (unit: m)
#define sqr(x) ((x)*(x))

double calc_earth_dis(double long_1, double lat_1, double long_2, double lat_2)
{
    double          radLat1, radLat2;
    double          a, b, s;

    radLat1 = lat_1*M_PI/180.0;
    radLat2 = lat_2*M_PI/180.0;

    a = radLat1 - radLat2;
    b = (long_1 - long_2)*M_PI/180.0;

    s = 2*asin( sqrt(sqr(sin(a/2)) + cos(radLat1)*cos(radLat2)*sqr(b/2)) );
    s = s*EARTH_RADIUS;
    //s = round(s*10000)/10000.0;

    return s;
}



std::vector<double> get_bounding_box(std::vector<GSLAM::Point3d> poly){
    double max_Y = -1e6, min_Y = 1e6, max_X = -1e6, min_X = 1e6;
    for(auto point:poly){
        if (point.y < min_Y)
            min_Y = point.y;
        if (point.y > max_Y)
            max_Y = point.y;
        if (point.x < min_X)
            min_X = point.x;
        if (point.x > max_X)
            max_X = point.x;
    }
    return {min_X, min_Y, max_X, max_Y};
}

std::vector<GSLAM::Point3d> get_local_map(std::vector<GSLAM::Point3d> poly, GSLAM::Point3d origin){
    std::vector<GSLAM::Point3d> ret;
    //double dx = origin.distanceTo({origin.latitude(), origin.longitude()+0.001,origin.altitude()})*1000;
    //double dy = origin.distanceTo({origin.latitude()+0.001, origin.longtitude(),origin.altitude()})*1000;
    double dx = calc_earth_dis(origin.x,origin.y,origin.x+0.001,origin.y)*1000;

    double dy = calc_earth_dis(origin.x,origin.y,origin.x,origin.y+0.001)*1000;

    //  std::cout<<dx<<" "<<dy<<std::endl;

    for(auto point: poly)
        ret.push_back({(point.x-origin.x)*dx,
                       (point.y-origin.y)*dy,
                       point.z});
    return ret;
}

std::vector<GSLAM::Point3d> get_global_from_local(std::vector<GSLAM::Point3d> local2dmap, GSLAM::Point3d origin){
    std::vector<GSLAM::Point3d> ret;
    //double dx = origin.distanceTo({origin.y, origin.x+0.001,origin.z})*1000;
    //double dy = origin.distanceTo({origin.y+0.001, origin.x,origin.z})*1000;

    double dx = calc_earth_dis(origin.x,origin.y,origin.x+0.001,origin.y)*1000;

    double dy = calc_earth_dis(origin.x,origin.y,origin.x,origin.y+0.001)*1000;
    for(auto point: local2dmap)
        ret.push_back({origin.x+point.x/dx,origin.y+point.y/dy, point.z});
    return ret;
}

bool get_intersection(GSLAM::Point3d a, GSLAM::Point3d b,
                      GSLAM::Point3d c, GSLAM::Point3d d,
                      GSLAM::Point3d& output){
    GSLAM::Point2d normal_line1 = {b.y - a.y, a.x-b.x};
    GSLAM::Point2d normal_line2 = {d.y - c.y, c.x-d.x};
    double denominator = normal_line1.x*normal_line2.y - normal_line1.y*normal_line2.x;
    if(std::abs(denominator) < 1e-6) return false;
    double distC_N2 = normal_line2.x * c.x + normal_line2.y*c.y;
    double distA_N2 = normal_line2.x * a.x + normal_line2.y*a.y - distC_N2;
    double distB_N2 = normal_line2.x * b.x + normal_line2.y*b.y - distC_N2;
    if ( distA_N2 * distB_N2 >=0) return false;
    double distA_N1 = normal_line1.x * a.x + normal_line1.y * a.y;
    double distC_N1 = normal_line1.x * c.x + normal_line1.y * c.y - distA_N1;
    double distD_N1 = normal_line1.x * d.x + normal_line1.y * d.y - distA_N1;
    if (distC_N1*distD_N1 >=0) return false;
    double fraction = distA_N2 / denominator;
    double dx = fraction * normal_line1.y;
    double dy = -fraction*normal_line1.x;
    output.x = a.x+dx;
    output.y = a.y+dy;
    output.z = a.z;
    return true;
}

double get_gap_from_camera_overlap(double image_focal, double image_width,double fly_height,double overlap){
    return image_width / image_focal * fly_height * ( 1.0 -overlap);
}
// rotation origin mast be same to local map origin
std::vector<GSLAM::Point3d> transform_points(std::vector<GSLAM::Point3d> points, double angle){
    std::vector<GSLAM::Point3d> transformed_local_poly;
    for(auto point: points)
        transformed_local_poly.push_back({
                                             point.x*cos(angle) - point.y*sin(angle),
                                             point.x*sin(angle) + point.y*cos(angle),
                                             point.z});
    return transformed_local_poly;
}

std::vector<GSLAM::Point3d> ploy_to_path(std::vector<GSLAM::Point3d> poly, GSLAM::Point3d landing_point, double gap, double angle)
{
    int id = 0;
    //  assert(poly.size() > 2);
    std::vector<GSLAM::Point3d> local_poly = get_local_map(poly, landing_point);
    local_poly = transform_points(local_poly, angle);
    //min_X, min_Y, max_X, max_Y
    std::vector<double> boundingbox = get_bounding_box(local_poly);

    std::vector<GSLAM::Point3d> local_waypoints;
    for (double current_y = boundingbox[1] + gap/2.0; current_y < boundingbox[3]; current_y += gap)
    {
        std::vector<GSLAM::Point3d> intersection_point_left_right;
        for (size_t i = 0; i < local_poly.size(); i++)
        {
            size_t end_index = (i == local_poly.size()-1) ? 0 : i+1;
            GSLAM::Point3d intersection_point;
            if(get_intersection({boundingbox[0], current_y,local_poly[0].z},
            {boundingbox[2], current_y,local_poly[0].z},
                                local_poly[i],local_poly[end_index],
                                intersection_point))
                intersection_point_left_right.push_back(intersection_point);
        }
        //    assert(intersection_point_left_right.size() == 2);
        //        qDebug()<<intersection_point_left_right[0].x<<" || "<<intersection_point_left_right[1].x;
        if (intersection_point_left_right[0].x > intersection_point_left_right[1].x)
            std::swap(intersection_point_left_right[0],intersection_point_left_right[1]);
        local_waypoints.push_back(intersection_point_left_right[0]);
        local_waypoints.push_back(intersection_point_left_right[1]);
    }

    // always start from left bottom
    for (size_t i = 2; i < local_waypoints.size(); i += 4)
        std::swap(local_waypoints[i],local_waypoints[i+1]);

    local_waypoints = transform_points(local_waypoints, -angle);
    std::vector<GSLAM::Point3d> gps_waypoints = get_global_from_local(local_waypoints, landing_point);
    std::vector<GSLAM::Point3d> ret;
    ret.push_back(landing_point);
    for(auto gps_waypoint:gps_waypoints)
        ret.push_back(gps_waypoint);
    ret.push_back(landing_point);
    return ret;
}
